/**
 * @file ebex_gyro_sc.c
 *
 * @date Sep 27, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <memory_util.h>
#include <log_util.h>
#include <ebex_util.h>

#include <ebex_math.h>
#include <ebex_matrix.h>
#include <ebex_vector.h>
#include <ebex_quaternion.h>

#include <data_file.h>
#include <dirfile_data.h>

#include <ebex_filter_cfg.h>
#include <ebex_statistics.h>
#include <ebex_genetic_stats.h>
#include <ebex_model.h>
#include <ebex_genetic_chromosome.h>

#include <ebex_gyro_sc.h>

const size_t num_gyro_params = 6;

static size_t ebex_gyro_sc_ensure_buffer(data_source_t *m_this, size_t m_num)
{

	if (m_num > m_this->buffer_size)
		ebex_data_resize(m_this, m_num);

	if (m_num > m_this->buffer_produced - m_this->buffer_consumed)
	{
		ebex_data_fill_buffer(m_this, m_num);
	}

	return m_this->buffer_produced - m_this->buffer_consumed;

}

static bool ebex_gyro_sc_fill_sc(ebex_gyro_sc_t *m_this, size_t m_current_segment)
{
	data_source_t *source = m_this->sc_source[0];
	ebex_3vec_t euler;
	size_t i;

	m_this->sc_segment[m_current_segment].sc_index = 0;

	for (i = 1; i < m_this->num_sc; i++)
	{
		///@TODO: Fix so that two star cameras can have the same timestamp
		data_source_t *putative = m_this->sc_source[i];
		if ((putative->buffer_produced > putative->buffer_consumed)
				&& (putative->timebuffer[putative->buffer_consumed] < source->timebuffer[source->buffer_consumed]))
		{
			source = putative;
			m_this->sc_segment[m_current_segment].sc_index = i;
		}
	}

	if (source->buffer_produced <= source->buffer_consumed)
	{
		elog(log_level_normal, "Exhausted all SC readings");
		return false;
	}

	m_this->sc_segment[m_current_segment].time = source->timebuffer[source->buffer_consumed];
	if (filter_config.sc_files[m_this->sc_segment[m_current_segment].sc_index][3].units == DATA_UNIT_MS)
	{
		m_this->sc_segment[m_current_segment].time /= 100.0;
	}

	m_this->sc_segment[m_current_segment].time += filter_config.sc[m_this->sc_segment[m_current_segment].sc_index].time_lag;

	/// We force roll to be [-PI, PI]
	euler.roll = source->databuffer[0][source->buffer_consumed];
	if (filter_config.sc_files[m_this->sc_segment[m_current_segment].sc_index][0].units == DATA_UNIT_DEGREES)
		euler.roll *= M_PI / 180.0;
	while (euler.roll < -M_PI) euler.roll += M_2PI;
	while (euler.roll >= M_PI) euler.roll -= M_2PI;

	/// Pitch is [-PI/2,PI/2] but does not wrap around
	euler.pitch = source->databuffer[1][source->buffer_consumed];
	if (filter_config.sc_files[m_this->sc_segment[m_current_segment].sc_index][1].units == DATA_UNIT_DEGREES)
		euler.pitch *= M_PI / 180.0;
	if (euler.pitch < -M_PI_2 || euler.pitch > M_PI_2)
	{
		dief("Invalid pitch value %f", euler.pitch);
		return false;
	}
	euler.yaw = source->databuffer[2][source->buffer_consumed];
	if (filter_config.sc_files[m_this->sc_segment[m_current_segment].sc_index][2].units == DATA_UNIT_DEGREES)
		euler.yaw *= M_PI / 180.0;
	euler.yaw = fmod(euler.yaw, M_2PI);
	if (euler.yaw < 0.0) euler.yaw += M_2PI;
	ebex_quat_from_euler(&m_this->sc_segment[m_current_segment].attitude,
			&euler, rotation_order_XYZ);

	/**
	 * Deal with uncertainties
	 */
	/// We force roll to be [-PI, PI]
	euler.roll = source->databuffer[3][source->buffer_consumed];
	if (filter_config.sc_files[m_this->sc_segment[m_current_segment].sc_index][4].units == DATA_UNIT_DEGREES)
		euler.roll *= M_PI / 180.0;
	while (euler.roll < -M_PI) euler.roll += M_2PI;
	if (euler.roll < 0.0) euler.roll = -euler.roll;
	while (euler.roll >= M_PI) euler.roll -= M_2PI;

	/// Pitch is [-PI/2,PI/2] but does not wrap around
	euler.pitch = source->databuffer[4][source->buffer_consumed];
	if (filter_config.sc_files[m_this->sc_segment[m_current_segment].sc_index][5].units == DATA_UNIT_DEGREES)
		euler.pitch *= M_PI / 180.0;
	if (euler.pitch < 0.0) euler.pitch = -euler.pitch;
	if (euler.pitch > M_PI_2)
	{
		dief("Invalid pitch value %f", euler.pitch);
		return false;
	}
	euler.yaw = source->databuffer[5][source->buffer_consumed];
	if (filter_config.sc_files[m_this->sc_segment[m_current_segment].sc_index][6].units == DATA_UNIT_DEGREES)
		euler.yaw *= M_PI / 180.0;
	euler.yaw = fmod(euler.yaw, M_2PI);
	if (euler.yaw < 0.0) euler.yaw += M_2PI;
	ebex_quat_from_euler(&m_this->sc_segment[m_current_segment].uncertainty,
			&euler, rotation_order_XYZ);

	elog(log_level_verbose, "Produced SC reading at time %f, Roll:%0.10f, Pitch:%0.10f, Yaw:%0.10f",
			m_this->sc_segment[m_current_segment].time,
			ebex_quat_get_roll(&m_this->sc_segment[m_current_segment].attitude),
			ebex_quat_get_pitch(&m_this->sc_segment[m_current_segment].attitude),
			ebex_quat_get_yaw(&m_this->sc_segment[m_current_segment].attitude));

	source->buffer_consumed++;
	return true;
}

static bool ebex_gyro_sc_consume_sc(ebex_gyro_sc_t *m_this)
{
	if (!m_this->used_segments)
	{
		die("Invalid consumption call!");
	}

	memmove(m_this->sc_segment, m_this->sc_segment + 1, (m_this->used_segments - 1) * sizeof(ebex_sc_reading_t));
	return ebex_gyro_sc_fill_sc(m_this, m_this->used_segments - 1);
}

static bool ebex_gyro_sc_fill_gyro(ebex_gyro_sc_t *m_this, size_t m_current_segment)
{
	size_t req_gyroreadings;

	double sc_time;
	data_source_t *gyrobox_src;
	ebex_gyro_reading_t *gyrobox_dest;

	double bandwidth = 0.0;
	e_kernel_type kernel = filter_config.smoothing_kernel;

	if (kernel <= kernel_type_none || kernel >= kernel_type_tblEnd)
		kernel = kernel_type_epanechnikov;

	if (m_this->sc_segment[m_current_segment + 1].time < m_this->sc_segment[m_current_segment].time)
	{
		dief("Star Camera times are not consecutive.  Got %f after %f",m_this->sc_segment[m_current_segment + 1].time, m_this->sc_segment[m_current_segment].time);
		return false;
	}
	/// Here, we pick a first guess of how many gyro readings we will need for the SC segment
	req_gyroreadings = (size_t)
			ceil((m_this->sc_segment[m_current_segment + 1].time - m_this->sc_segment[m_current_segment].time)
			* filter_config.gyro_rate);

	for (size_t box = 0; box < m_this->num_gyrobox; box++)
	{
		sc_time = m_this->sc_segment[m_current_segment].time;
		if (filter_config.gyro_files[box][3].units == DATA_UNIT_MS)
		{
			sc_time = sc_time * 100.0;
		}

		gyrobox_src = m_this->gyrobox_source[box];
		gyrobox_dest = &m_this->gyro_segment[box][m_current_segment];

		if (ebex_gyro_sc_ensure_buffer(gyrobox_src, req_gyroreadings) < req_gyroreadings)
		{
			elog(log_level_normal, "Reached end of file %s at time %f", gyrobox_src->datafile->filename, gyrobox_src->timebuffer[gyrobox_src->buffer_consumed]);
			return false;
		}

		/// Check the gyro rate to ensure it is reasonable for this box
		if (!ISDBL_TINY((gyrobox_src->buffer_produced - gyrobox_src->buffer_consumed - 1) /
				(gyrobox_src->timebuffer[gyrobox_src->buffer_produced - 1] - gyrobox_src->timebuffer[gyrobox_src->buffer_consumed])
				 - filter_config.gyro_rate))
		{
			dief("Mismatch between measured gyro rate %f and configuration gyro rate %f",
					(gyrobox_src->buffer_produced - gyrobox_src->buffer_consumed - 1) /
							(gyrobox_src->timebuffer[gyrobox_src->buffer_produced - 1] - gyrobox_src->timebuffer[gyrobox_src->buffer_consumed]),
					filter_config.gyro_rate);
			return false;
		}

		/// Rewind the buffer to find a time before the star camera reading
		while (gyrobox_src->timebuffer[gyrobox_src->buffer_consumed] > sc_time)
		{
			if (!gyrobox_src->buffer_consumed)
			{
				elog(log_level_warning, "Gyro data is later than SC data.  Discarding SC solution for time %f", m_this->sc_segment[m_current_segment - 1].time);
				return false;
			}
			gyrobox_src->buffer_consumed--;
		}

		/// Now, we move to the position immediately following the star camera reading
		while (!ISDBL_TINY(gyrobox_src->timebuffer[gyrobox_src->buffer_consumed] - sc_time - 1.0 / filter_config.gyro_rate))
		{
			if (++gyrobox_src->buffer_consumed >= gyrobox_src->buffer_produced)
			{
				if (ebex_gyro_sc_ensure_buffer(gyrobox_src, req_gyroreadings) < req_gyroreadings)
				{
					elog(log_level_normal, "Reached end of file %s at time %f", gyrobox_src->datafile->filename, gyrobox_src->timebuffer[gyrobox_src->buffer_consumed]);
					return false;
				}
			}
		}

		/// We've possibly changed buffer_consumed, so re-check for sufficient data
		if (ebex_gyro_sc_ensure_buffer(gyrobox_src, req_gyroreadings) < req_gyroreadings)
		{
			elog(log_level_normal, "Reached end of file %s at time %f", gyrobox_src->datafile->filename, gyrobox_src->timebuffer[gyrobox_src->buffer_consumed]);
			return false;
		}
		/// Set the number of required readings such that there is one past the the second star camera reading
		sc_time = m_this->sc_segment[m_current_segment + 1].time;
		if (filter_config.gyro_files[box][3].units == DATA_UNIT_MS)
		{
			sc_time = sc_time * 100.0;
		}
		while (gyrobox_src->timebuffer[gyrobox_src->buffer_consumed + req_gyroreadings - 1]
		                               - sc_time > 1.0 / filter_config.gyro_rate)
			req_gyroreadings--;
		while (sc_time > gyrobox_src->timebuffer[gyrobox_src->buffer_consumed + req_gyroreadings - 1])
			req_gyroreadings++;


		gyrobox_dest->num_obs = ++req_gyroreadings;
		gyrobox_dest->time = e_memalign(req_gyroreadings * sizeof(double));
		if (filter_config.gyro_files[box][3].units == DATA_UNIT_MS)
		{
			for (size_t reading = 0; reading < req_gyroreadings; reading++)
			{
				gyrobox_dest->time[reading] = gyrobox_src->timebuffer[gyrobox_src->buffer_consumed + reading] / 100.0;
			}
		}
		else
		{
			memcpy(gyrobox_dest->time, &gyrobox_src->timebuffer[gyrobox_src->buffer_consumed], req_gyroreadings * sizeof(double));
		}
		elog(log_level_verbose, "Starting Gyro reading of %zu elements at time %f", req_gyroreadings, gyrobox_dest->time[0]);

		gyrobox_dest->observation = e_memalign(req_gyroreadings * sizeof(ebex_3vec_t));

		if (filter_config.smooth_gyros)
		{
			if (filter_config.smoothing_bandwidth <= 1.0 / filter_config.gyro_rate)
			{
				bandwidth = 2.0 / filter_config.gyro_rate;
			}
			else
			{
				bandwidth = filter_config.smoothing_bandwidth;
			}

			req_gyroreadings += (size_t) (4.0 * bandwidth * filter_config.gyro_rate);
		}

		if (ebex_gyro_sc_ensure_buffer(gyrobox_src, req_gyroreadings) < req_gyroreadings)
		{
			elog(log_level_normal, "Reached end of file %s at time %f", gyrobox_src->datafile->filename, gyrobox_src->timebuffer[gyrobox_src->buffer_consumed]);
			return false;
		}

		if (filter_config.output_parent && box == 0)
		{
			ebex_gyro_sc_ensure_buffer(m_this->parent_source, req_gyroreadings);
			while (!ISDBL_TINY(
					m_this->parent_source->timebuffer[m_this->parent_source->buffer_consumed] -
					gyrobox_src->timebuffer[gyrobox_src->buffer_consumed]))
					m_this->parent_source->buffer_consumed++;
			ebex_gyro_sc_ensure_buffer(m_this->parent_source, req_gyroreadings);
			gyrobox_dest->parent = e_memalign(req_gyroreadings * sizeof(ebex_3vec_t));

			for (size_t j = 0; j < gyrobox_dest->num_obs; j++)
			{
				gyrobox_dest->parent[j].x = m_this->parent_source->databuffer[0][m_this->parent_source->buffer_consumed + j];
				gyrobox_dest->parent[j].y = m_this->parent_source->databuffer[1][m_this->parent_source->buffer_consumed + j];
				gyrobox_dest->parent[j].z = m_this->parent_source->databuffer[2][m_this->parent_source->buffer_consumed + j];

			}
		}

		/**
		 * If we requested a kernel smoothing, then use that routine to copy data over
		 * to the observation buffer.  Otherwise, perform a straight copy
		 */
		if (filter_config.smooth_gyros)
		{
			double *robust_weight = e_calloc(3 * req_gyroreadings, sizeof(double));
			double *residual = e_calloc(3 * req_gyroreadings, sizeof(double));

#			pragma omp parallel for shared(gyrobox_dest, robust_weight, residual) schedule(static)
			for (size_t gyro = 0; gyro < 3; gyro++)
			{
			ebex_stats_lowess(gyrobox_src->timebuffer + gyrobox_src->buffer_consumed,
					gyrobox_src->databuffer[gyro] + gyrobox_src->buffer_consumed,
					gyrobox_dest->num_obs, bandwidth * filter_config.gyro_rate, 5, 0.1,
					&(gyrobox_dest->observation[0].element[gyro]), EBEX_3VEC_STRIDE,
					robust_weight + (gyro * req_gyroreadings), residual + (gyro * req_gyroreadings));
			}

			e_free(robust_weight);
			e_free(residual);

//			ebex_stats_smooth(gyrobox_src->buffer_consumed + req_gyroreadings,
//				gyrobox_src->databuffer[0], 1,
//				gyrobox_src->timebuffer, 1,
//				bandwidth, kernel,
//				gyrobox_dest->num_obs,
//				gyrobox_dest->time, 1,
//				&(gyrobox_dest->observation[0].x), EBEX_3VEC_STRIDE);
//			ebex_stats_smooth(gyrobox_src->buffer_consumed + req_gyroreadings,
//				gyrobox_src->databuffer[1], 1,
//				gyrobox_src->timebuffer, 1,
//				bandwidth, kernel,
//				gyrobox_dest->num_obs,
//				gyrobox_dest->time, 1,
//				&(gyrobox_dest->observation[0].y), EBEX_3VEC_STRIDE);
//			ebex_stats_smooth(gyrobox_src->buffer_consumed + req_gyroreadings,
//				gyrobox_src->databuffer[2], 1,
//				gyrobox_src->timebuffer, 1,
//				bandwidth, kernel,
//				gyrobox_dest->num_obs,
//				gyrobox_dest->time, 1,
//				&(gyrobox_dest->observation[0].z), EBEX_3VEC_STRIDE);


		}
		else
		{
			for (size_t j = 0; j < gyrobox_dest->num_obs; j++)
			{
				gyrobox_dest->observation[j].x = gyrobox_src->databuffer[0][gyrobox_src->buffer_consumed + j];
				gyrobox_dest->observation[j].y = gyrobox_src->databuffer[1][gyrobox_src->buffer_consumed + j];
				gyrobox_dest->observation[j].z = gyrobox_src->databuffer[2][gyrobox_src->buffer_consumed + j];

			}
		}

		/**
		 * Handle negating various gyros
		 */
		for (size_t k = 0; k < 3; k++)
		{
			if (filter_config.gyro_box[box].gyro[k].is_inverted)
			{
				for (size_t j = 0; j < gyrobox_dest->num_obs; j++)
				{
					gyrobox_dest->observation[j].element[k] = -gyrobox_dest->observation[j].element[k];
				}
			}
		}

		/**
		 * Handle unit conversions for the gyros
		 */
		for (size_t k = 0; k < 3; k++)
		{
			if (filter_config.gyro_files[box][k].units == DATA_UNIT_DEGREES)
			{
				for (size_t j = 0; j < gyrobox_dest->num_obs; j++)
				{
					gyrobox_dest->observation[j].element[k] = gyrobox_dest->observation[j].element[k] * M_PI / 180.0;
				}
			}
		}
		/// We don't fully consume the buffer as we need to leave space for the next segment overlap
		gyrobox_src->buffer_consumed += (gyrobox_dest->num_obs - MIN(gyrobox_dest->num_obs, MAX(4, (size_t)round(4.0 * bandwidth * filter_config.gyro_rate))));

	}

	return true;
}

static size_t ebex_gyro_sc_produce(ebex_gyro_sc_t *m_this, size_t m_num)
{
	size_t current_segment;
	size_t to_produce = m_num;
	size_t retval = 0;

	if (m_this->num_segments - m_this->used_segments < m_num)
	{
		m_this->sc_segment = e_realloc(m_this->sc_segment, (m_this->num_segments + m_num + 1) * sizeof(ebex_sc_reading_t));
		for (size_t i = 0; i < m_this->num_gyrobox; i++)
		{
			m_this->gyro_segment[i] = e_realloc(m_this->gyro_segment[i], (m_this->num_segments + m_num) * sizeof(ebex_gyro_reading_t));
			e_memset(m_this->gyro_segment[i] + m_this->num_segments, 0, m_num * sizeof(ebex_gyro_reading_t));
		}
		m_this->num_segments += m_num;
	}
	current_segment = m_this->used_segments;

	/// Fill the Star camera buffer first as we know exactly how many readings we need
	for (size_t i = 0; i < m_this->num_sc; i++)
	{
		to_produce = MIN(to_produce, ebex_gyro_sc_ensure_buffer(m_this->sc_source[i], m_num + 1));
	}

	/// If we don't have any segments loaded yet, we need a star camera reading to bracket gyro data
	if (!current_segment && (!ebex_gyro_sc_fill_sc(m_this, 0)))
	{
		die("Error while filling Star Camera readings");
		return 0;
	}

	while (to_produce--)
	{

		if (! ebex_gyro_sc_fill_sc(m_this, current_segment + 1))
		{
			die("Error while filling Star Camera readings");
			return 0;
		}

		/// Try to fill the gyro buffers.  If we have gappy data, discard a star camera solution and try again
		if (! ebex_gyro_sc_fill_gyro(m_this, current_segment))
		{
			if (!ebex_gyro_sc_consume_sc(m_this))
			{
				die("Error while consuming Star Camera reading");
				return 0;
			}
			m_num++;
			continue;
		}

		current_segment = ++(m_this->used_segments);
		retval++;
	}

	return retval;
}

static size_t ebex_gyro_sc_consume(ebex_gyro_sc_t *m_this, size_t m_num)
{
	size_t num_to_consume = MIN(m_num, m_this->used_segments - 1); /// Always leave at least 1 segment in the hopper
	size_t retval = num_to_consume;

	if (!m_this->used_segments) return 0;

	while (num_to_consume--)
	{
		for (size_t i = 0; i < m_this->num_gyrobox; i++)
		{
			e_free(m_this->gyro_segment[i]->observation);
			e_free_quiet(m_this->gyro_segment[i]->parent);
			memmove(m_this->gyro_segment[i], m_this->gyro_segment[i] + 1, (m_this->used_segments - 1) * sizeof(ebex_gyro_reading_t));
			e_memset(m_this->gyro_segment[i] + m_this->used_segments - 1, 0, sizeof(ebex_gyro_reading_t));
		}
		/// We always have one extra star camera reading (bracketing)
		memmove(m_this->sc_segment, m_this->sc_segment + 1, (m_this->used_segments) * sizeof(ebex_sc_reading_t));
		e_memset(m_this->sc_segment + m_this->used_segments, 0, sizeof(ebex_sc_reading_t));
		m_this->used_segments--;
	}

	elog(log_level_verbose, "Consumed %zu entries", retval);
	return retval;
}

static bool ebex_gyro_sc_reset(ebex_gyro_sc_t *m_this)
{

	m_this->used_segments = 0;

	for (size_t i = 0; i < filter_config.num_sc; i++)
	{
		if (!ebex_data_reset_source(m_this->sc_source[i])) return false;
	}

	for (size_t box = 0; box < filter_config.num_gyros; box++)
	{
		if (!ebex_data_reset_source(m_this->gyrobox_source[box])) return false;
	}
	return true;
}

ebex_gyro_sc_t *ebex_gyro_sc_new(void)
{
	ebex_gyro_sc_t *retval = e_calloc(1,sizeof(ebex_gyro_sc_t));

	retval->produce = ebex_gyro_sc_produce;
	retval->consume = ebex_gyro_sc_consume;
	retval->reset = ebex_gyro_sc_reset;

	return retval;
}
bool ebex_gyro_sc_add_parent_src(ebex_gyro_sc_t *m_group, data_source_t *m_parent)
{

	if (!m_group || !m_parent)
	{
		die("Passed NULL pointer!");
		return false;
	}

	if (m_group->parent_source)
	{
		die("Cannot re-assign parent source!");
		return false;
	}

	m_group->parent_source = m_parent;

	return true;
}

bool ebex_gyro_sc_add_gyrobox_src(ebex_gyro_sc_t *m_group, data_source_t *m_gyrobox)
{

	if (!m_group || !m_gyrobox)
	{
		die("Passed NULL pointer!");
		return false;
	}

	m_group->gyrobox_source = e_realloc(m_group->gyrobox_source, (m_group->num_gyrobox + 1) * sizeof(data_source_t*));
	m_group->gyrobox_source[m_group->num_gyrobox] = m_gyrobox;

	m_group->gyro_segment = e_realloc(m_group->gyro_segment, (m_group->num_gyrobox + 1) * sizeof(ebex_gyro_reading_t*));
	m_group->gyro_segment[m_group->num_gyrobox] = NULL;

	m_group->num_gyrobox++;
	return true;
}

bool ebex_gyro_sc_add_sc_src(ebex_gyro_sc_t *m_group, data_source_t *m_sc)
{

	if (!m_group || !m_sc)
	{
		die("Passed NULL pointer!");
		return false;
	}

	m_group->sc_source = e_realloc(m_group->sc_source, (m_group->num_sc + 1) * sizeof(data_source_t*));
	m_group->sc_source[m_group->num_sc] = m_sc;

	m_group->num_sc++;

	return true;
}

/**
 * Verifies the GYRO_SC organism.  Double parameters are checked for invalid data and then "bounced" when they overrun their limits,
 * preventing a local minimum at the limit point.
 * Quaternion parameters are checked for invalid data and then normalized.
 * @param m_pop Population Pointer
 * @param m_entity Entity to check
 * @return True if the organism is valid.  False if the parameters are NaN
 */
bool ebex_gyro_sc_organism_verify(population_t *m_pop, organism_t *m_entity)
{
	ebex_model_t *model = (ebex_model_t*)m_pop->data;

	for (size_t i = 0; i < num_gyro_params; i++)
	{
		for (size_t j = 0; j < model->num_gyroboxes; j++)
		{
			for (size_t k = 0; k < 3; k++)
			{
				size_t offset = j * 3 + k;
				if (isnan(m_entity->chromosome[i].dbl_allele[offset])) return false;

				if (m_entity->chromosome[i].allele_max_double > m_entity->chromosome[i].allele_min_double)
				{
					if (m_entity->chromosome[i].dbl_allele[offset] >  m_entity->chromosome[i].allele_max_double)
						m_entity->chromosome[i].dbl_allele[offset] = m_entity->chromosome[i].allele_max_double -
							(m_entity->chromosome[i].dbl_allele[offset]-m_entity->chromosome[i].allele_max_double);
					if (m_entity->chromosome[i].dbl_allele[offset] <  m_entity->chromosome[i].allele_min_double)
						m_entity->chromosome[i].dbl_allele[offset] = m_entity->chromosome[i].allele_min_double -
							(m_entity->chromosome[i].dbl_allele[offset]-m_entity->chromosome[i].allele_min_double);
				}
				else
				{
					m_entity->chromosome[i].dbl_allele[offset] = m_entity->chromosome[i].allele_max_double;
				}
			}
		}
	}

	for (size_t i = num_gyro_params; i < m_pop->num_chromosomes; i++)
	{
		if (ebex_quat_isnan(m_entity->chromosome[i].q_allele)) return false;
		ebex_quat_normalize(m_entity->chromosome[i].q_allele, m_entity->chromosome[i].q_allele);
	}

	return true;
}

bool ebex_gyro_sc_generate_chromosome(population_t *m_pop, organism_t *m_entity)
{

	ebex_model_t *model = (ebex_model_t*)m_pop->data;

	ebex_genetic_chromosome_double_allocate(m_pop, m_entity, num_gyro_params, 3 * model->num_gyroboxes);
	ebex_genetic_chromosome_quaternion_allocate(m_pop, m_entity, model->num_gyroboxes + model->num_sc, 1);

	for (size_t i = 0; i < num_gyro_params; i++)
	{
		/// Doubles
		m_entity->chromosome[i].allele_max_double = filter_config.gyro_box[0].gyro[0].param.max.array[i];
		m_entity->chromosome[i].allele_min_double = filter_config.gyro_box[0].gyro[0].param.min.array[i];
		m_entity->chromosome[i].allele_mutation_prob = filter_config.gyro_box[0].gyro[0].param.mutation_likelihood.array[i];
		memcpy(&m_entity->chromosome[i].rnd, &filter_config.gyro_box[0].gyro[0].param.sample.array[i], sizeof(m_entity->chromosome[i].rnd));

		for (size_t j = 0; j < model->num_gyroboxes; j++)
		{
			size_t offset = j + num_gyro_params;

			m_entity->chromosome[offset].user_data = &model->gyrobox[j];
			m_entity->chromosome[offset].allele_mutation_prob = model->gyrobox[j].mutation_likelihood;
			memcpy(&m_entity->chromosome[offset].rnd, &model->gyrobox[j].alignment_noise, sizeof(m_entity->chromosome[offset].rnd));
			ebex_quat_copy(m_entity->chromosome[offset].q_allele, &model->gyrobox[j].alignment);

			for (size_t k = 0; k < 3; k++)
			{
				size_t allele_offset = j * 3 + k;
				m_entity->chromosome[i].dbl_allele[allele_offset] = model->gyrobox[j].gyro[k].param.value.array[i];
			}
		}
	}

	for (size_t i = 0; i < model->num_sc; i++)
	{
		size_t offset = i + num_gyro_params + model->num_gyroboxes;
		m_entity->chromosome[offset].user_data = &model->sc[i];
		m_entity->chromosome[offset].allele_mutation_prob = model->sc[i].mutation_likelihood;
		memcpy(&m_entity->chromosome[offset].rnd, &model->sc[i].alignment_noise, sizeof(random_sample_t));
		ebex_quat_copy(m_entity->chromosome[offset].q_allele, &model->sc[i].alignment);
	}

	return true;
}

bool ebex_gyro_sc_register_uncertainty_output(data_source_t **m_output, const char *m_filename, const data_file_types_t m_type)
{
	data_file_t *new_file = NULL;
	data_source_t *new_source = NULL;
	dirfile_data_param_t param = {.spf = 1, .stride = 1};

	if (m_type != DATA_FILE_NONE)
	{
		if (m_type != DATA_FILE_DIRFILE) die("Currently only DIRFILE output is supported");

		if (!(*m_output))
		{
			new_source = ebex_data_new_source(1024);
			if (!(new_file = ebex_data_set_timefile(new_source, m_filename, m_type, 0, true)))
			{
				dief("Could not add time to the %s datasource", m_filename);
				return false;
			}
			param.field = "time";
			ebex_data_set_parameters(new_file, &param);
			*m_output = new_source;
		}

		new_source = *m_output;
		for (int i = 0; gyro_sc_uncertainty_output_lookup_table[i].position != gyro_sc_uncertainty_output_tblEnd; i++)
		{
			if (!(new_file = ebex_data_add_datafile(new_source, new_source->timefile->filename, m_type, 0, true)))
			{
				dief("Could not add %s to the %s datasource", gyro_sc_uncertainty_output_lookup_table[i].text, new_source->timefile->filename);
				return false;
			}

			param.field = gyro_sc_uncertainty_output_lookup_table[i].text;
			ebex_data_set_parameters(new_file, &param);
		}
	}
	return true;
}

bool ebex_gyro_sc_register_param_output(data_source_t **m_output, const char *m_filename, const data_file_types_t m_type)
{
	data_file_t *new_file = NULL;
	data_source_t *new_source = NULL;
	dirfile_data_param_t param = {.spf = 1, .stride = 1};

	if (m_type != DATA_FILE_NONE)
	{
		if (m_type != DATA_FILE_DIRFILE) die("Currently only DIRFILE output is supported");

		if (!(*m_output))
		{
			new_source = ebex_data_new_source(1024);
			if (!(new_file = ebex_data_set_timefile(new_source, m_filename, m_type, 0, true)))
			{
				dief("Could not add time to the %s datasource", m_filename);
				return false;
			}
			param.field = "time";
			ebex_data_set_parameters(new_file, &param);
			*m_output = new_source;
		}

		new_source = *m_output;
		for (int i = 0; gyro_sc_param_output_lookup_table[i].position != gyro_sc_param_output_tblEnd; i++)
		{
			if (!(new_file = ebex_data_add_datafile(new_source, new_source->timefile->filename, m_type, 0, true)))
			{
				dief("Could not add %s to the %s datasource", gyro_sc_param_output_lookup_table[i].text, new_source->timefile->filename);
				return false;
			}

			param.field = gyro_sc_param_output_lookup_table[i].text;
			ebex_data_set_parameters(new_file, &param);
		}
	}
	return true;
}

bool ebex_gyro_sc_register_attitude_output(ebex_model_t *m_model, data_source_t **m_output, const char *m_filename, const data_file_types_t m_type)
{
	data_file_t *new_file = NULL;
	data_source_t *new_source = NULL;
	dirfile_data_param_t param = {.spf = 1, .stride = 1};

	if (m_type != DATA_FILE_NONE)
	{
		if (m_type != DATA_FILE_DIRFILE) die("Currently only DIRFILE output is supported");

		if (!(*m_output))
		{
			new_source = ebex_data_new_source(1024);
			if (!(new_file = ebex_data_set_timefile(new_source, m_filename, m_type, 0, true)))
			{
				dief("Could not add time to the %s datasource", m_filename);
				return false;
			}
			param.field = "time";
			ebex_data_set_parameters(new_file, &param);
			*m_output = new_source;
		}

		new_source = *m_output;
		for (int i = 0; gyro_sc_attitude_output_lookup_table[i].position != gyro_sc_attitude_output_tblEnd; i++)
		{
			if (!(new_file = ebex_data_add_datafile(new_source, new_source->timefile->filename, m_type, 0, true)))
			{
				dief("Could not add %s to the %s datasource", gyro_sc_attitude_output_lookup_table[i].text, new_source->timefile->filename);
				return false;
			}

			param.field = gyro_sc_attitude_output_lookup_table[i].text;
			ebex_data_set_parameters(new_file, &param);
		}
	}
	return true;
}

bool ebex_gyro_sc_register_parent_output(ebex_model_t *m_model, data_source_t **m_output, const char *m_filename, const data_file_types_t m_type)
{
	data_file_t *new_file = NULL;
	data_source_t *new_source = NULL;
	dirfile_data_param_t param = {.spf = 1, .stride = 1};

	if (m_type != DATA_FILE_NONE)
	{
		if (m_type != DATA_FILE_DIRFILE) die("Currently only DIRFILE output is supported");

		if (!(*m_output))
		{
			new_source = ebex_data_new_source(1024);
			if (!(new_file = ebex_data_set_timefile(new_source, m_filename, m_type, 0, true)))
			{
				dief("Could not add time to the %s datasource", m_filename);
				return false;
			}
			param.field = "time";
			ebex_data_set_parameters(new_file, &param);
			*m_output = new_source;
		}

		new_source = *m_output;
		for (int i = 0; gyro_sc_parent_output_lookup_table[i].position != gyro_sc_parent_output_tblEnd; i++)
		{
			if (!(new_file = ebex_data_add_datafile(new_source, new_source->timefile->filename, m_type, 0, true)))
			{
				dief("Could not add %s to the %s datasource", gyro_sc_parent_output_lookup_table[i].text, new_source->timefile->filename);
				return false;
			}

			param.field = gyro_sc_parent_output_lookup_table[i].text;
			ebex_data_set_parameters(new_file, &param);
		}
	}
	return true;
}

size_t ebex_gyro_sc_output_time(data_source_t *m_file, double *m_time, size_t m_num)
{
	size_t retval;
	retval = ebex_data_copy_time_to_buffer(m_file, m_time, m_num);
	if (retval != m_num) elog(log_level_warning, "Missing output!  Tried to log %zu time elements to %s but only got %zu", m_num, m_file->timefile->filename, retval);

	return retval;
}

size_t ebex_gyro_sc_output_parent(data_source_t *m_file, const ebex_3vec_t *m_data, size_t m_num)
{

	for (size_t i = 0; i < m_num; i++)
	{
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_parent_output_lookup_table[gyro_sc_parent_output_parent_AZ].text, &m_data->yaw, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_parent_output_lookup_table[gyro_sc_parent_output_parent_EL].text, &m_data->pitch, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_parent_output_lookup_table[gyro_sc_parent_output_parent_ROLL].text, &m_data->roll, 1);
		m_file->buffer_produced++;
	}
	m_file->buffer_produced -= m_num;

	return m_num;
}

size_t ebex_gyro_sc_output_attitude(data_source_t *m_file, ebex_quat_t *m_quat, const ebex_3vec_t *m_ref, const ebex_quat_t *m_rotation, size_t m_num)
{
	ebex_3vec_t ypr;
	ebex_quat_t tmp_quat;

	for (size_t i = 0; i < m_num; i++)
	{
		if (m_rotation) ebex_quat_mul(&tmp_quat, &m_quat[i], m_rotation);
		else ebex_quat_copy(&tmp_quat, &m_quat[i]);
		assert(!isnan(tmp_quat.w));

		ebex_3vec_euler_from_quat(&ypr, rotation_order_XYZ, &tmp_quat);
		if (m_ref) ebex_3vec_make_compatible_euler(&ypr, m_ref);
		else
		{
			ypr.yaw = fmod(ypr.yaw, M_2PI);
			if (ypr.yaw < 0.0) ypr.yaw += M_2PI;
		}
		assert(!isnan(ypr.yaw));
		assert(!isnan(ypr.pitch));
		assert(!isnan(ypr.roll));
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_attitude_output_lookup_table[gyro_sc_attitude_output_yaw].text, &ypr.yaw, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_attitude_output_lookup_table[gyro_sc_attitude_output_pitch].text, &ypr.pitch, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_attitude_output_lookup_table[gyro_sc_attitude_output_roll].text, &ypr.roll, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_attitude_output_lookup_table[gyro_sc_attitude_output_q1].text, &tmp_quat.x, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_attitude_output_lookup_table[gyro_sc_attitude_output_q2].text, &tmp_quat.y, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_attitude_output_lookup_table[gyro_sc_attitude_output_q3].text, &tmp_quat.z, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_attitude_output_lookup_table[gyro_sc_attitude_output_q4].text, &tmp_quat.w, 1);
		m_file->buffer_produced++;
	}
	m_file->buffer_produced -= m_num;

	return m_num;
}

size_t ebex_gyro_sc_output_params(data_source_t *m_file, gyrobox_t *m_gyrobox, sc_param_t *sc_param, const ebex_3vec_t *m_ref, size_t m_num)
{
	ebex_3vec_t sc_ypr;

	ebex_3vec_euler_from_quat(&sc_ypr, rotation_order_XYZ, &sc_param->alignment);
	if (m_ref) ebex_3vec_make_compatible_euler(&sc_ypr, m_ref);
	else
	{
		sc_ypr.yaw = fmod(sc_ypr.yaw, M_2PI);
		if (sc_ypr.yaw < 0.0) sc_ypr.yaw += M_2PI;
	}

	for (size_t i = 0; i < m_num; i++)
	{
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy1_asymmetric_scaling].text, &m_gyrobox->gyro[0].param.value.asym_scaling, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy2_asymmetric_scaling].text, &m_gyrobox->gyro[1].param.value.asym_scaling, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy3_asymmetric_scaling].text, &m_gyrobox->gyro[2].param.value.asym_scaling, 1);

		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy1_symmetric_scaling].text, &m_gyrobox->gyro[0].param.value.sym_scaling, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy2_symmetric_scaling].text, &m_gyrobox->gyro[1].param.value.sym_scaling, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy3_symmetric_scaling].text, &m_gyrobox->gyro[2].param.value.sym_scaling, 1);

		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy1_beta].text, &m_gyrobox->gyro[0].param.value.beta, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy2_beta].text, &m_gyrobox->gyro[1].param.value.beta, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy3_beta].text, &m_gyrobox->gyro[2].param.value.beta, 1);

		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy1_bias].text, &m_gyrobox->gyro[0].param.value.bias, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy2_bias].text, &m_gyrobox->gyro[1].param.value.bias, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy3_bias].text, &m_gyrobox->gyro[2].param.value.bias, 1);

		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy1_misalignment].text, &m_gyrobox->gyro[0].param.value.misalignment, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy2_misalignment].text, &m_gyrobox->gyro[1].param.value.misalignment, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_gy3_misalignment].text, &m_gyrobox->gyro[2].param.value.misalignment, 1);

		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_sc_roll].text, &sc_ypr.roll, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_sc_pitch].text, &sc_ypr.pitch, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_param_output_lookup_table[gyro_sc_param_output_sc_yaw].text, &sc_ypr.yaw, 1);
		m_file->buffer_produced++;
	}
	m_file->buffer_produced -= m_num;

	return m_num;
}

size_t ebex_gyro_sc_output_uncertainties(data_source_t *m_file, ebex_mat3x3_t *m_cov, size_t m_num)
{
	for (size_t i = 0; i < m_num; i++)
	{
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_xx].text, &m_cov->col[0].x, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_yx].text, &m_cov->col[0].y, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_zx].text, &m_cov->col[0].z, 1);

		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_xy].text, &m_cov->col[1].x, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_yy].text, &m_cov->col[1].y, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_zy].text, &m_cov->col[1].z, 1);

		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_xz].text, &m_cov->col[2].x, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_yz].text, &m_cov->col[2].y, 1);
		ebex_data_copy_to_buffer_by_dirfile_field(m_file, gyro_sc_uncertainty_output_lookup_table[gyro_sc_uncertainty_output_C_zz].text, &m_cov->col[2].z, 1);
		m_file->buffer_produced++;
	}
	m_file->buffer_produced -= m_num;

	return m_num;
}

/**
 * Propagates an attitude forward through a gyrodata segment of time.  The intermediate
 * steps are (potentially) saved for later use as is the final attitude.
 * @param m_model The overall model to use
 * @param m_gyrodata Pointer to the gyro readings structure
 * @param m_output_attitude Pointer to hold the final output quaternion
 * @param m_gyrobox Pointer to the gyro parameters
 * @param m_intermediate_quat Pointer that is at least #m_gyrodata->num_obs long to hold all intermediate steps (optional)
 * @param m_offset Pointer to the gyrodata offset information that would create a zero-error result (optional)
 * @param m_ratenoise Pointer to the pre-computed noise data for the gyros (optional)
 * @param m_segment Number of the segment to use for computation
 * @return Total number of gyro observations used
 */
size_t ebex_gyro_sc_quat_propagate_forward(ebex_model_t *m_model, ebex_gyro_reading_t *m_gyrodata,
											 ebex_quat_t *m_output_attitude, gyrobox_t *m_gyrobox,
											 ebex_quat_t *m_intermediate_quat, size_t m_intermediate_step_size,
											 ebex_3vec_t *m_offset,
											 ebex_3vec_t *m_ratenoise, size_t m_segment)
{
	double partial_step;
	double gyro_time_step = 1.0 / m_model->parameters->gyro_rate;
	ebex_3vec_t offset = EBEX_3VEC_ZERO;
	ebex_3vec_t tmp_gyro;
	size_t step = 0;
	bool use_noise = m_ratenoise ? true : false;

	if (m_offset) ebex_3vec_copy(&offset, m_offset);

	/// First step is a partial from SC to first gyro observation
	partial_step = m_gyrodata->time[0] - (m_model->data->sc_segment[m_segment].time + gyro_time_step);
	if (partial_step > DBL_TINY)
	{
		ebex_gyro_step(m_output_attitude, m_output_attitude, &m_gyrodata->observation[0], m_ratenoise++, m_gyrobox, partial_step, use_noise);
	}

	/**
	 * The gyro observation records the previous gyro step rate information, so we step until we are definitely past the
	 * second star camera observation
	 */
	while(m_gyrodata->time[step] - m_model->data->sc_segment[m_segment + 1].time < DBL_TINY)
	{
		ebex_3vec_sub(&tmp_gyro, &m_gyrodata->observation[step], &offset);
		ebex_gyro_step(m_output_attitude, m_output_attitude, &tmp_gyro, m_ratenoise++, m_gyrobox, gyro_time_step, use_noise );
		if (m_intermediate_quat)
		{
			ebex_quat_copy(&m_intermediate_quat[step * m_intermediate_step_size], m_output_attitude);
		}
		step++;
	}

	/**
	 *  We don't consider the final (partial) step in our intermediate solution, only for comparison to the star camera
	 */
	partial_step = m_model->data->sc_segment[m_segment + 1].time - (m_gyrodata->time[step] + gyro_time_step);
	if (partial_step > DBL_TINY)
	{
		ebex_3vec_sub(&tmp_gyro, &m_gyrodata->observation[step], &offset);
		ebex_gyro_step(m_output_attitude, m_output_attitude, &tmp_gyro, m_ratenoise++, m_gyrobox, partial_step, use_noise);
	}

	return step;
}

void ebex_gyro_sc_quat_propagate_backward(ebex_model_t *model, ebex_gyro_reading_t *m_gyrodata,
											 ebex_quat_t *m_output_attitude, gyrobox_t *m_gyrobox,
											 ebex_quat_t *m_intermediate_quat, size_t m_intermediate_step_size,
											 ebex_3vec_t *m_offset, ebex_3vec_t *m_ratenoise,
											 size_t m_segment, size_t m_numsteps)
{
	double partial_step;
	double gyro_time_step = 1.0 / model->parameters->gyro_rate;
	ebex_3vec_t offset = EBEX_3VEC_ZERO;
	ebex_3vec_t tmp_gyro;
	bool use_noise = m_ratenoise ? true : false;

	if (m_offset) ebex_3vec_copy(&offset, m_offset);

	--m_numsteps;
	if (m_intermediate_quat) ebex_quat_copy(&m_intermediate_quat[(m_numsteps) * m_intermediate_step_size], m_output_attitude);

	partial_step = m_gyrodata->time[m_numsteps] - model->data->sc_segment[m_segment + 1].time + gyro_time_step;
	if (partial_step > DBL_TINY)
	{
		ebex_3vec_sub(&tmp_gyro, &m_gyrodata->observation[m_numsteps], &offset);
		ebex_gyro_step_backward(m_output_attitude, m_output_attitude, &tmp_gyro, m_ratenoise++, m_gyrobox, partial_step, use_noise);
		if (m_intermediate_quat)
		{
			ebex_quat_copy(&m_intermediate_quat[(m_numsteps - 1) * m_intermediate_step_size], m_output_attitude);
		}
		m_numsteps--;
	}

	do
	{
		ebex_3vec_sub(&tmp_gyro, &m_gyrodata->observation[m_numsteps], &offset);
		ebex_gyro_step_backward(m_output_attitude, m_output_attitude, &tmp_gyro, m_ratenoise++, m_gyrobox, gyro_time_step, use_noise );
		if (m_intermediate_quat)
		{
			ebex_quat_copy(&m_intermediate_quat[(m_numsteps - 1) * m_intermediate_step_size], m_output_attitude);
		}
	}while (--m_numsteps);

}

void ebex_state_print_stats(population_t *m_pop)
{
	ebex_3vec_t best_guess;
	ebex_mat3x3_t rotation_matrix;
	ebex_mat3x3_t misalignment_matrix;
	ebex_quat_t inv_quat;
	double mean[3];
	double min[3];
	double max[3];
	double var[3];
	double mean_error;
	ebex_model_t *model = (ebex_model_t*)m_pop->data;

	for (int i = 0; i < num_gyro_params; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			genetic_allele_double_statistics(m_pop,i,j,max, min, mean, NULL, var, NULL, NULL, NULL);
			elog(log_level_verbose, "%s: %s min %f, max %f, mean %f, var %f, best %f",
			     model->parameters->gyro_box[0].gyro[j].name, gyro_param_lookup_table[i].text, min[0], max[0], mean[0], var[0],
			     m_pop->sorted_organisms[0]->chromosome[i].dbl_allele[j]);
		}
	}

	genetic_allele_quaternion_statistics(m_pop, num_gyro_params, 0, max, min, mean, NULL, var, NULL, NULL, NULL);
	ebex_3vec_euler_from_quat(&best_guess, rotation_order_XYZ, m_pop->sorted_organisms[0]->chromosome[num_gyro_params].q_allele);
	elog(log_level_verbose, "Gyro box Yaw min %f, max %f, mean %f, var %f, best %f", min[2], max[2], mean[2], var[2], best_guess.z);
	elog(log_level_verbose, "Gyro box Pitch min %f, max %f, mean %f, var %f, best %f", min[1], max[1], mean[1], var[1], best_guess.y);
	elog(log_level_verbose, "Gyro box Roll min %f, max %f, mean %f, var %f, best %f", min[0], max[0], mean[0], var[0], best_guess.x);

	genetic_allele_quaternion_statistics(m_pop, num_gyro_params + 1, 0, max, min, mean, NULL, var, NULL, NULL, NULL);
	ebex_3vec_euler_from_quat(&best_guess, rotation_order_XYZ, m_pop->sorted_organisms[0]->chromosome[num_gyro_params + 1].q_allele);
	elog(log_level_verbose, "SC box Yaw min %f, max %f, mean %f, var %f, best %f", min[2], max[2], mean[2], var[2], best_guess.z);
	elog(log_level_verbose, "SC box Pitch min %f, max %f, mean %f, var %f, best %f", min[1], max[1], mean[1], var[1], best_guess.y);
	elog(log_level_verbose, "SC box Roll min %f, max %f, mean %f, var %f, best %f", min[0], max[0], mean[0], var[0], best_guess.x);

	mean_error = 180.0 / M_PI * (M_PI - m_pop->sorted_organisms[0]->fitness);
	if (mean_error > 1.0)
	{
		elog(log_level_verbose, "After bootstrap, best solution has mean error of %1.4f degrees", mean_error);
	}
	else if (mean_error > 1.0 / 60.0)
	{
		elog(log_level_verbose, "After bootstrap, best solution has mean error of %1.4f\'", mean_error * 60.0);
	}
	else
	{
		elog(log_level_verbose, "After bootstrap, best solution has mean error of %1.4f\"", mean_error * 3600.0);
	}
	ebex_matrix_from_quat(&rotation_matrix, m_pop->sorted_organisms[0]->chromosome[num_gyro_params + 1].q_allele);
	elog(log_level_verbose, "After bootstrap, best estimate of SC matrix is");
	elog(log_level_verbose, "\nB = [[%1.9f %1.9f %1.9f]\n[%1.9f %1.9f %1.9f]\n[%1.9f %1.9f %1.9f]]\n",
			rotation_matrix.col[0].x, rotation_matrix.col[1].x, rotation_matrix.col[2].x,
			rotation_matrix.col[0].y, rotation_matrix.col[1].y, rotation_matrix.col[2].y,
			rotation_matrix.col[0].z, rotation_matrix.col[1].z, rotation_matrix.col[2].z);

	ebex_quat_inv(&inv_quat, m_pop->sorted_organisms[0]->chromosome[num_gyro_params + 1].q_allele);
	ebex_matrix_from_quat(&rotation_matrix, &inv_quat);

	ebex_3mat_set_col(&misalignment_matrix, 0, (ebex_3vec_t[]){{1.0, 0.0, 0.0}});
	ebex_3mat_set_col(&misalignment_matrix, 1, (ebex_3vec_t[]){{m_pop->sorted_organisms[0]->chromosome[3].dbl_allele[(3 * model->parameters->reference_gyrobox) + 2], 1.0, 0.0}});
	ebex_3mat_set_col(&misalignment_matrix, 2, (ebex_3vec_t[]){{m_pop->sorted_organisms[0]->chromosome[3].dbl_allele[(3 * model->parameters->reference_gyrobox) + 1],
		m_pop->sorted_organisms[0]->chromosome[3].dbl_allele[(3 * model->parameters->reference_gyrobox)], 1.0}});
	ebex_3mat_mul(&rotation_matrix, &rotation_matrix, &misalignment_matrix);

	elog(log_level_verbose, "After bootstrap, best estimate of rotation matrix 'M' [SC] = [M].[Gyro]' is");
	elog(log_level_verbose, "\nB = [[%1.9f %1.9f %1.9f]\n[%1.9f %1.9f %1.9f]\n[%1.9f %1.9f %1.9f]]\n",
			rotation_matrix.col[0].x, rotation_matrix.col[1].x, rotation_matrix.col[2].x,
			rotation_matrix.col[0].y, rotation_matrix.col[1].y, rotation_matrix.col[2].y,
			rotation_matrix.col[0].z, rotation_matrix.col[1].z, rotation_matrix.col[2].z);
}

/**
 * Returns the probability of an evolved gyro solution being correct, given size of a throw and combined variance for two star camera
 * readings.
 * @param m_sc SC Quaternion at the end of a throw
 * @param m_gyro Evolved quaternion from the beginning
 * @param m_gyrobox Pointer to the gyrobox structure containing gyro noise information
 * @param m_samples Number of samples over which the gyros have been evolved
 * @param m_scvariance Combined variance of both SC readings
 * @return Probability (0.0 - 1.0) of the reading being correct
 */
double ebex_gyro_sc_get_probability(ebex_quat_t *m_sc, ebex_quat_t *m_gyro, gyrobox_t *m_gyrobox, size_t m_samples, double m_gyrorate, double m_scvariance)
{
	ebex_quat_t err_quat;
	double radians_err;
	double probability;
	double expected_sigma;

	expected_sigma = sqrt( (pow(m_gyrobox->gyro[0].param.sample.rate_noise.gaussian.stddev / m_gyrorate, 2.0)
						+ pow(m_gyrobox->gyro[1].param.sample.rate_noise.gaussian.stddev / m_gyrorate, 2.0)
						+ pow(m_gyrobox->gyro[2].param.sample.rate_noise.gaussian.stddev / m_gyrorate, 2.0))	* (double)m_samples
						+ m_scvariance);

	ebex_quat_error(&err_quat, m_gyro, m_sc);
	if (isnan(err_quat.w))
	{
		elog(log_level_debug, "got NaN!");
	}
	radians_err = 2.0 * acos(err_quat.w);
	if (radians_err > DBL_EPSILON)
		probability = erfc(radians_err / (expected_sigma * sqrt(2)));
	else
		probability = 1.0;

	return probability;
}
